{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "try:\n",
    "  # We must install required packages if we are in Google Colab\n",
    "  import google.colab\n",
    "  %pip install roboticstoolbox-python>=1.0.2\n",
    "except:\n",
    "  # We are not in Google Colab\n",
    "  # Apply custon style to notebook\n",
    "  from IPython.core.display import HTML\n",
    "  import pathlib\n",
    "  styles_path = pathlib.Path(pathlib.Path().absolute(), \"style\", \"style.css\")\n",
    "  styles = open(styles_path, \"r\").read()\n",
    "  HTML(f\"<style>{styles}</style>\")"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 5.0 Manipulator Performance Measures\n",
    "\n",
    "$\\large{\\text{Manipulator Differential Kinematics}} \\\\ \\large{\\text{Part I: Kinematics, Velocity, and Applications}}$\n",
    "\n",
    "$\\text{By Jesse Haviland and Peter Corke}$\n",
    "\n",
    "\n",
    "<br>\n",
    "\n",
    "The sections of the Tutorial paper related to this notebook are listed next to each contents entry.\n",
    "It is beneficial to read these sections of the paper before attempting the notebook Section.\n",
    "\n",
    "### Contents\n",
    "\n",
    "[5.1 Manipulability Measure](#m)\n",
    "* Manipulator Performance Metrics\n",
    "\n",
    "[5.2 Condition Number](#cond)\n",
    "* Manipulator Performance Metrics\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "# We will do the imports required for this notebook here\n",
    "\n",
    "# numpy provides import array and linear algebra utilities\n",
    "import numpy as np\n",
    "\n",
    "# the robotics toolbox provides robotics specific functionality\n",
    "import roboticstoolbox as rtb"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Manipulator performance metrics seek to quantify the performance of a manipulator in a given configuration. In this section, we explore two common manipulator performance metrics based on the manipulator Jacobian.\n",
    "\n",
    "Things to note\n",
    "* The metrics are unitless, and the upper bound of a metric depends on the manipulator kinematic model (i.e. joint types and link lengths). Consequently, metrics computed for different manipulators are not directly comparable.\n",
    "* The manipulator Jacobian contains three rows corresponding to translational rates, and three rows corresponding to angular rates. Therefore, any metric using the whole Jacobian will produce a non-homogeneous result due to the mixed units. Depending on the manipulator scale, this can cause either the translational or rotational component to dominate the result. This problem also arises in manipulators with mixed prismatic and revolute joints. In general, the most intuitive use of performance metrics comes from using only the translational or rotational rows of the manipulator Jacobian (where the choice of which depends on the use case), and only using the metric on a manipulator comprising with a single joint type."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='m'></a>\n",
    "\n",
    "### 5.1 Manipulability Measure\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The Yoshikawa manipulability index is the most widely used and accepted performance metric. The index is calculated as\n",
    "\n",
    "\\begin{align*}\n",
    "    m(\\bf{q}) = \\sqrt{\n",
    "        \\text{det}\n",
    "        \\Big(\n",
    "            \\hat{\\bf{J}}(\\bf{q})\n",
    "            \\hat{\\bf{J}}(\\bf{q})^\\top\n",
    "        \\Big)\n",
    "    }\n",
    "\\end{align*}\n",
    "\n",
    "where $\\hat{\\bf{J}}(\\bf{q}) \\in \\mathbb{R}^{3 \\times n}$ is either the translational or rotational rows of $\\bf{J}(\\bf{q})$ causing  $m(\\bf{q})$ to describe the corresponding component of manipulability. \n",
    "Note that Yoshikawa used $\\bf{J}(\\bf{q})$ instead of $\\hat{\\bf{J}}(\\bf{q})$ in his original paper but we describe it so due to limitations of Jacobian based measures discussed above.\n",
    "The scalar $m(\\bf{q})$ describes the volume of a 3-dimensional ellipsoid -- if this ellipsoid is close to spherical, then the manipulator can achieve any arbitrary end-effector (translational or rotational depending on $\\hat{\\bf{J}}(\\vec{q})$) velocity. The ellipsoid is described by three radii aligned with its principal axes. A small radius indicates the robot's inability to achieve a velocity in the corresponding direction. At a singularity, the ellipsoid's radius becomes zero along the corresponding axis and the volume becomes zero. If the manipulator's configuration is well conditioned, these ellipsoids will have a larger volume. Therefore, the manipulability index is essentially a measure of how easily a manipulator can achieve an arbitrary velocity."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We first make a method to calculate the manipulability of a robot using the above description."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "def manipulability(robot, q, axes):\n",
    "    \"\"\"\n",
    "    Calculates the manipulability of the robot at joint configuration q\n",
    "\n",
    "    :robot: A Robot object to find the manipulability of\n",
    "    :q: The joint coordinates of the robot (ndarray)\n",
    "    :axes: A boolean list which correspond with the Cartesian axes to\n",
    "        find the manipulability of (6 boolean values in a list)\n",
    "    \"\"\"\n",
    "    # calculate the Jacobian\n",
    "    J = robot.jacobe(q)\n",
    "\n",
    "    # only keep the selected axes of J\n",
    "    J = J[axes, :]\n",
    "\n",
    "    # calculate the manipulability\n",
    "    m = np.sqrt(np.linalg.det(J @ J.T))\n",
    "\n",
    "    return m"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We make a robot and define some axes selections. The lists `trans_axes`, `rot_axes`, `all_axes` each correspond to a set of Cartesian axes."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Make a Panda\n",
    "panda = rtb.models.Panda()\n",
    "\n",
    "# Only the translation axes\n",
    "trans_axes = [True, True, True, False, False, False]\n",
    "\n",
    "# Only the rotation aces\n",
    "rot_axes = [False, False, False, True, True, True]\n",
    "\n",
    "# All axes\n",
    "all_axes = [True, True, True, True, True, True]"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Lets test out our method using the Panda robot in the ready `panda.qr` configuration."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Translational manipulability: 0.14384031993097537\n",
      "Rotational manipulability: 2.745582183072283\n",
      "All axes manipulability: 0.08375150968113343\n"
     ]
    }
   ],
   "source": [
    "print(f\"Translational manipulability: {manipulability(panda, panda.qr, trans_axes)}\")\n",
    "print(f\"Rotational manipulability: {manipulability(panda, panda.qr, rot_axes)}\")\n",
    "print(f\"All axes manipulability: {manipulability(panda, panda.qr, all_axes)}\")"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In the `panda.qr` configuration, the Panda is well configured. That is to say that it is in a manipulable configuration and can easily move in any direction.\n",
    "\n",
    "For the Panda, 0.14 is around the peak of the translational manipulability and 2.74 is in the upper range of the rotational manipulability. These two numbers are not comparable to each other due to being made from different units. We also can not compare these numbers to translational/rotational manipulabilities on other robots.\n",
    "\n",
    "We can also see that the rotational manipulability is an order of magnitude greater than the translational manipulability. This difference can cause unintuitive results in the all axes manipulability measure."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In the next cell, we can see a test the manipulability of the Panda in a poorly configured pose `panda.qz`. This is the zero joint angle configuration in which the Panda is in a singularity."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Translational manipulability: 0.009904977685365267\n",
      "Rotational manipulability: nan\n",
      "All axes manipulability: nan\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "/var/folders/jm/65py5sf558zb166cykvzz9zw0000gq/T/ipykernel_50710/2288985554.py:17: RuntimeWarning: invalid value encountered in sqrt\n",
      "  m = np.sqrt(np.linalg.det(J @ J.T))\n"
     ]
    }
   ],
   "source": [
    "print(f\"Translational manipulability: {manipulability(panda, panda.qz, trans_axes)}\")\n",
    "print(f\"Rotational manipulability: {manipulability(panda, panda.qz, rot_axes)}\")\n",
    "print(f\"All axes manipulability: {manipulability(panda, panda.qz, all_axes)}\")"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<br>\n",
    "\n",
    "<a id='cond'></a>\n",
    "\n",
    "### 5.2 Condition Number\n",
    "---"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The condition number of the manipulator Jacobian was proposed as a performance measure. The condition number is\n",
    "\n",
    "\\begin{align*}\n",
    "    \\kappa =\n",
    "    \\dfrac{\\sigma_{\\text{max}}}\n",
    "    {\\sigma_{\\text{min}}}\n",
    "    \\in [1, \\ \\infty ]\n",
    "\\end{align*}\n",
    "\n",
    "where $\\sigma_{\\text{max}}$ and $\\sigma_{\\text{min}}$ are the maximum and minimum singular values of $\\hat{\\bf{J}}(\\bf{q})$ respectively. The condition number is a measure of velocity isotropy. A condition number close to $1$ means that the manipulator can achieve a velocity in a direction equally as easily as any other direction. However, a high condition number does not guarantee a high manipulability index where the manipulator may struggle to move in all directions."
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We first make a method to calculate the condition number of a robot using the above description."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "def cond(robot, q, axes):\n",
    "    \"\"\"\n",
    "    Calculates the condition number of the robot's Jacobian at\n",
    "    joint configuration q\n",
    "\n",
    "    :robot: A Robot object to find the manipulability of\n",
    "    :q: The joint coordinates of the robot (ndarray)\n",
    "    :axes: A boolean list which correspond with the Cartesian axes to\n",
    "        find the manipulability of (6 boolean values in a list)\n",
    "    \"\"\"\n",
    "    # calculate the Jacobian\n",
    "    J = robot.jacobe(q)\n",
    "\n",
    "    # only keep the selected axes of J\n",
    "    J = J[axes, :]\n",
    "\n",
    "    _, s, _ = np.linalg.svd(J)\n",
    "\n",
    "    Jcond = np.max(s) / np.min(s)\n",
    "\n",
    "    ## Note: we could just use the cond method from numpy\n",
    "    # Jcond = np.linalg.cond(J)\n",
    "\n",
    "    return Jcond"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Lets test out our method using the Panda robot in the ready `panda.qr` configuration."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Translational condition number: 2.5512647717700543\n",
      "Rotational condition number: 2.031042407741737\n",
      "All axes condition number: 8.910974536808437\n"
     ]
    }
   ],
   "source": [
    "print(f\"Translational condition number: {cond(panda, panda.qr, trans_axes)}\")\n",
    "print(f\"Rotational condition number: {cond(panda, panda.qr, rot_axes)}\")\n",
    "print(f\"All axes condition number: {cond(panda, panda.qr, all_axes)}\")"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Lets test out our method using the Panda robot in the ready `panda.qz` configuration."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Translational condition number: 4.866993326664893\n",
      "Rotational condition number: 1.0400617828738614e+16\n",
      "All axes condition number: 6.968343580985044e+16\n"
     ]
    }
   ],
   "source": [
    "print(f\"Translational condition number: {cond(panda, panda.qz, trans_axes)}\")\n",
    "print(f\"Rotational condition number: {cond(panda, panda.qz, rot_axes)}\")\n",
    "print(f\"All axes condition number: {cond(panda, panda.qz, all_axes)}\")"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Remember that a condition number of 1 is perfect and infinity is the worst."
   ]
  }
 ],
 "metadata": {
  "interpreter": {
   "hash": "528815e074ebcdb9b34bcb695d4aa9d425bdb2cc6656b4ca45050b51a4125937"
  },
  "kernelspec": {
   "display_name": "Python 3.8.10 ('rtb')",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.11.0"
  },
  "orig_nbformat": 4
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
